#include "SerialESP8266wifi.h"

/////////////////////////////////////////////////////////////////////////////////////////////
const char OK[] = "OK";
const char FAIL[] = "FAIL";
const char ERROR[] = "ERROR";

const char CWMODE_1[] = "AT+CWMODE=1";      // Station 模式
const char CWMODE_3[] = "AT+CWMODE=3";      // SoftAP + Station 模式
const char CWSAP[] = "AT+CWSAP=\"";         // 设置 ap参数

const char CONNECT[] = "CONNECT";
const char CLOSED[] = "CLOSED";

const char EOL[] = "\n";
const char COMMA[] = ",";
const char COMMA_1[] = "\",";
const char COMMA_2[] = "\",\"";
/////////////////////////////////////////////////////////////////////////////////////////////

SerialESP8266wifi::SerialESP8266wifi(Stream& serialIn, Stream& serialOut, int resetPin)
    : SerialESP8266wifi(serialIn, serialOut, resetPin, nullptr) {
}

SerialESP8266wifi::SerialESP8266wifi(Stream& serialIn, Stream& serialOut, int resetPin, Stream* dbgSerial) {
    _serialIn = &serialIn;
    _serialOut = &serialOut;
    _resetPin = resetPin;

    pinMode(_resetPin, OUTPUT);
    digitalWrite(_resetPin, LOW);

    flags.connectToServerUsingTCP = true;
    flags.endSendWithNewline = true;
    flags.started = false;
    flags.localServerConfigured = false;
    flags.localApConfigured = false;
    flags.apConfigured = false;
    flags.serverConfigured = false;

    _dbgSerial = dbgSerial;
    flags.echoOnOff = true;

    _connection.channel = 0;
    _connection.connected = false;
    _connection.trycount = 0;
    recvcache.init(CACHE_BUFF_MAX);
}

void niceDelay(int duration) {
    unsigned long startMillis = millis();
    while (millis() - startMillis < duration) {
        sqrt(4700);
    }
}

void SerialESP8266wifi::endSendWithNewline(bool endSendWithNewline) {
    flags.endSendWithNewline = endSendWithNewline;
}

//------------------------------------------------------------------------------
const char READY[] = "ready";
const char NO_CHANGE[] = "no change";
const char ATE0[] = "ATE0";             // 打开echo
const char ATE1[] = "ATE1";             // 关闭echo
const char CIPMUX_0[] = "AT+CIPMUX=0";  // 允许1个连接
const char CIPMUX_1[] = "AT+CIPMUX=1";  // 允许多个连接
//------------------------------------------------------------------------------
int SerialESP8266wifi::begin() {
    flags.localServerConfigured = false;
    flags.localApConfigured = false;
    _connection.channel = 0;
    _connection.connected = false;
    _connection.trycount = 0;
    recvcache.clear();
    readcache[0] = 0;
    rdchpos = 0;
    rdmsginc = 0;
    flags.started = false;
    posipd = 0;
    ipdindex = -1;
    ipdsize = 0;

    bool statusOk = false;
    for (auto i = 0; i < 5; i++) {
        digitalWrite(_resetPin, LOW);
        niceDelay(500);
        digitalWrite(_resetPin, HIGH);
        statusOk = readCommand(3000, READY) == 1;   // 等待ready信号
        if (statusOk == true) {
            break;
        }
    }
    if (!statusOk) {
        return 1;
    }

    writeCommand(CWMODE_1, EOL);
    if (readCommand(1000, OK, NO_CHANGE) == 0) {
        return 2;
    }

    if (flags.echoOnOff) {
        writeCommand(ATE1, EOL);
    } else {
        writeCommand(ATE0, EOL);
    }
    if (readCommand(1000, OK, NO_CHANGE) == 0) {
        return 3;
    }

    writeCommand(CIPMUX_1, EOL);
    //writeCommand(CIPMUX_0, EOL);

    flags.started = readCommand(3000, OK, NO_CHANGE) > 0;
    return flags.started ? 0 : 4;
}

bool SerialESP8266wifi::isStarted() {
    return flags.started;
}

bool SerialESP8266wifi::restart() {
    return begin()
           && (!flags.localApConfigured || startLocalAp())
           && (!flags.localServerConfigured || startLocalServer())
           && (!flags.apConfigured || connectToAP())
           && (!flags.serverConfigured || connectToServer());
}

bool SerialESP8266wifi::connectToAP(const String& ssid, const String& password) {
    return connectToAP(ssid.c_str(), password.c_str());
}

bool SerialESP8266wifi::connectToAP(const char* ssid, const char* password) {
    strncpy(_ssid, ssid, sizeof _ssid);
    strncpy(_password, password, sizeof _password);
    flags.apConfigured = true;
    return connectToAP();
}

//------------------------------------------------------------------------------
const char CWJAP[] = "AT+CWJAP=\"";
const char DOUBLE_QUOTE[] = "\"";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::connectToAP() {
    writeCommand(CWJAP);
    _serialOut -> print(_ssid);
    writeCommand(COMMA_2);
    _serialOut -> print(_password);
    writeCommand(DOUBLE_QUOTE, EOL);

    readCommand(15000, OK, FAIL);
    return isConnectedToAP();
}

//------------------------------------------------------------------------------
const char CIFSR[] = "AT+CIFSR";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::isConnectedToAP() {
    writeCommand(CIFSR, EOL);
    return readCommand(350, OK, ERROR) == 1;
}

//------------------------------------------------------------------------------
const char STAIP[] = ":STAIP,\"";
const char STAMAC[] = ":STAMAC,\"";
//------------------------------------------------------------------------------
void SerialESP8266wifi::getIPMAC(char* ip, int maxip, int& lenip, char* mac, int mmac, int& lmac) {
    writeCommand(CIFSR, EOL);
    if (readCommand(5000, STAIP) == 1) {
        lenip = readBuffer(1000, nullptr, ip, maxip, '"').pos;
    }
    if (readCommand(5000, STAMAC) == 1) {
        lmac = readBuffer(1000, nullptr, mac, mmac, '"').pos;
    }
}

//------------------------------------------------------------------------------
const char CWLAP[] = "AT+CWLAP";
const char CWLAPV[] = "+CWLAP:";
//------------------------------------------------------------------------------
int SerialESP8266wifi::getSSIDs(WifiSSID* ssids, int slen, const char** names, int nlen) {
    if (ssids == nullptr || slen <= 0) {
        return 0;
    }
    char buff[65] = { 0 };
    String command;
    int index = 0;
    for (auto i = 0; i < nlen && index < slen; i++) {
        command = String(CWLAP) + "=\"" + names[i] + "\"";
        writeCommand(command.c_str(), EOL);
        if (readCommand(10000, CWLAPV) != 1 || recvcache.length() <= 0) {
            continue;
        }

        auto& info = *(ssids + index);
        do {
            auto fd = readBuffer(100, nullptr, buff, 64, '(');
            if (fd.find == false) {
                continue;
            }
            auto pos = readBuffer(100, nullptr, buff, 64, ',').pos;
            if (pos <= 0) {
                continue;
            }
            pos = readBuffer(100, nullptr, buff, 64, ',').pos;
            if (pos <= 0) {
                continue;
            }
            buff[pos] = 0;
            if (buff[pos - 1] == '"') {
                pos -= 1;
                buff[pos] = 0;
            }
            auto start = 0;
            if (buff[0] == '"') {
                start = 1;
                pos -= 1;
            }
            if (pos <= 0) {
                continue;
            }
            if (checkBuffer(buff + start, names[i], pos) == false) {
                continue;
            }
            info.ssid = names[i];
            pos = readBuffer(100, nullptr, buff, 64, ',').pos;
            if (pos > 0) {
                buff[pos] = 0;
                info.rssi = atoi(buff);
            }
            index++;
            showLog(String("ssid: ") + info.ssid + ", rssi: " + info.rssi);
        } while (0);
    }
    return index;
}

void SerialESP8266wifi::setTransportToUDP() {
    flags.connectToServerUsingTCP = false;
}

void SerialESP8266wifi::setTransportToTCP() {
    flags.connectToServerUsingTCP = true;
}

bool SerialESP8266wifi::connectToServer(String& ip, String& port) {
    return connectToServer(ip.c_str(), port.c_str());
}

bool SerialESP8266wifi::connectToServer(const char* ip, const char* port) {
    strncpy(_ip, ip, sizeof _ip);
    strncpy(_port, port, sizeof _port);
    flags.serverConfigured = true;
    return connectToServer();
}

//------------------------------------------------------------------------------
const char CIPSTART[] = "AT+CIPSTART=4,\"";
const char TCP[] = "TCP";
const char UDP[] = "UDP";
const char LINKED[] = "Linked";
const char ALREADY[] = "ALREAY";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::connectToServer() {
    writeCommand(CIPSTART);
    if (flags.connectToServerUsingTCP) {
        writeCommand(TCP);
    } else {
        writeCommand(UDP);
    }
    writeCommand(COMMA_2);
    _serialOut -> print(_ip);
    writeCommand(COMMA_1);
    _serialOut -> println(_port);

    _connection.connected = (readCommand(10000, LINKED, ALREADY) > 0);
    if (_connection.connected) {
        _connection.trycount = 0;
    }
    return _connection.connected;
}

//------------------------------------------------------------------------------
const char CIPCLOSE[] = "AT+CIPCLOSE=4";
//------------------------------------------------------------------------------
void SerialESP8266wifi::disconnectFromServer() {
    _connection.channel = 0;
    _connection.connected = false;
    _connection.trycount = 0;

    flags.serverConfigured = false;
    writeCommand(CIPCLOSE);
    readCommand(2000, OK);
}

bool SerialESP8266wifi::isConnectedToServer() {
    if (_connection.connected) {
        _connection.channel = 0;
        _connection.trycount = 0;
    }
    return _connection.connected;
}

bool SerialESP8266wifi::startLocalAPAndServer(const char* ssid, const char* password, const char* channel, const char* port) {
    strncpy(_localAPSSID, ssid, sizeof _localAPSSID);
    strncpy(_localAPPassword, password, sizeof _localAPPassword);
    strncpy(_localAPChannel, channel, sizeof _localAPChannel);
    strncpy(_localServerPort, port, sizeof _localServerPort);

    flags.localApConfigured = true;
    flags.localServerConfigured = true;
    return startLocalAp() && startLocalServer();
}

bool SerialESP8266wifi::startLocalAP(const char* ssid, const char* password, const char* channel) {
    strncpy(_localAPSSID, ssid, sizeof _localAPSSID);
    strncpy(_localAPPassword, password, sizeof _localAPPassword);
    strncpy(_localAPChannel, channel, sizeof _localAPChannel);

    flags.localApConfigured = true;
    return startLocalAp();
}

bool SerialESP8266wifi::startLocalServer(const char* port) {
    strncpy(_localServerPort, port, sizeof _localServerPort);
    flags.localServerConfigured = true;
    return startLocalServer();
}

//------------------------------------------------------------------------------
const char CIPSERVERSTART[] = "AT+CIPSERVER=1,";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::startLocalServer() {
    writeCommand(CIPSERVERSTART);
    _serialOut -> println(_localServerPort);
    flags.localServerRunning = (readCommand(2000, OK, NO_CHANGE) > 0);
    return flags.localServerRunning;
}

//------------------------------------------------------------------------------
const char THREE_COMMA[] = ",3";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::startLocalAp() {
    writeCommand(CWMODE_3, EOL);
    if (!readCommand(2000, OK, NO_CHANGE)) {
        return false;
    }
    writeCommand(CWSAP);
    _serialOut -> print(_localAPSSID);
    writeCommand(COMMA_2);
    _serialOut -> print(_localAPPassword);
    writeCommand(COMMA_1);
    _serialOut -> print(_localAPChannel);
    writeCommand(THREE_COMMA, EOL);

    flags.localApRunning = (readCommand(5000, OK, ERROR) == 1);
    return flags.localApRunning;
}

//------------------------------------------------------------------------------
const char CIPSERVERSTOP[] = "AT+CIPSERVER=0";
//------------------------------------------------------------------------------
bool SerialESP8266wifi::stopLocalServer() {
    writeCommand(CIPSERVERSTOP, EOL);
    auto stopped = (readCommand(2000, OK, NO_CHANGE) > 0);
    flags.localServerRunning = !stopped;
    flags.localServerConfigured = false; //to prevent autostart
    return stopped;
}

bool SerialESP8266wifi::stopLocalAP() {
    writeCommand(CWMODE_1, EOL);
    auto stopped = (readCommand(2000, OK, NO_CHANGE) > 0);
    flags.localApRunning = !stopped;
    flags.localApConfigured = false;
    return stopped;
}

bool SerialESP8266wifi::stopLocalAPAndServer() {
    return stopLocalAP() && stopLocalServer();
}

bool SerialESP8266wifi::isLocalAPAndServerRunning() {
    return flags.localApRunning & flags.localServerRunning;
}

bool SerialESP8266wifi::isLocalServerRunning() {
    return flags.localServerRunning;
}

void SerialESP8266wifi::watchdog() {
    if (flags.serverConfigured == true && _connection.connected == false) {
        _connection.trycount++;
        if (_connection.trycount >= 5) {
            restart();
        } else if (flags.apConfigured && !isConnectedToAP()) {
            if (!connectToAP()) {
                niceDelay(2000);
                if (!isConnectedToAP()) {
                    restart();
                }
            }
        } else {
            connectToServer();
        }
    }
}

//------------------------------------------------------------------------------
const char IPD[] = "+IPD,";
//------------------------------------------------------------------------------
// +IPD,0,2:xx
void SerialESP8266wifi::checkInput(bool log) {
    if (_serialIn == nullptr) {
        return;
    }
    watchdog();
    char c;

    while (_serialIn->available() && recvcache.space() > 0) {
        c = _serialIn->read();
        if (log == true) {
            showChar(c);
        }
        readcache[rdchpos++] = c;
        if (c == '+') {
            recvcache.write(readcache, rdchpos);
            rdchpos = 0;
            posipd = 1;
            continue;
        }
        posipd = (c == IPD[posipd]) ? posipd + 1 : 0;
        if (posipd >= 5) {
            ipdindex = 4;
            ipdsize = 0;
            posipd = 0;
            continue;
        } else if (ipdindex >= 0 && ipdsize == 0) {
            ipdindex++;
            if (c == ':') {
                for (auto i = ipdindex - 1; i > 0; i--) {
                    if (readcache[i] == ',') {
                        for (auto j = i + 1; j < ipdindex - 1; j++) {
                            ipdsize = ipdsize * 10 + (int)(readcache[j] - 0x30);
                        }
                        break;
                    }
                }
                if (ipdsize > MSG_BUFFER_MAX) {
                    ipdsize = MSG_BUFFER_MAX;
                }
            }
            continue;
        }

        if (ipdsize > 0) {
            ipdsize--;
            if (ipdsize <= 0) {
                if (callfunc != nullptr) {
                    WifiMessage message;
                    message.channel = readcache[4];
                    message.size = rdchpos - ipdindex;
                    message.message = &readcache[ipdindex];
                    callfunc(&message);
                }
                posipd = 0;
                ipdindex = -1;
                ipdsize = 0;
                rdchpos = 0;
            }
            continue;
        }

        if (rdchpos >= MSG_BUFFER_MAX) {
            recvcache.write(readcache, rdchpos);
            rdchpos = 0;
        }
        //sqrt(12345);    // delay microseconds 50
    }
}

void SerialESP8266wifi::registCall(cmdDealFunc func) {
    callfunc = func;
}

bool SerialESP8266wifi::checkBuffer(const char* in, const char* check, int inlen, int cklen, bool equal) {
    //showLog(String("checkBuffer: ") + (in ? in : "null") + " : " + inlen + " , " + (check ? check : "null") + " : " + cklen);
    if (in == nullptr || check == nullptr) {
        return false;
    }
    if (inlen <= 0) {
        inlen = strlen(in);
    }
    if (cklen <= 0) {
        cklen = strlen(check);
    }
    if (cklen > inlen) {
        return false;
    }
    if (equal == true && cklen != inlen) {
        return false;
    }
    for (int pos = 0; pos < cklen; pos++) {
        if (*(in + pos) != *(check + pos)) {
            return false;
        }
    }
    return true;
}

void SerialESP8266wifi::send(char channel, String& message) {
    if (message.length() <= 0) {
        return;
    }
    if (startSend(channel, message.length()) == false) {
        return;
    }
    endSend(channel, message);
}

//------------------------------------------------------------------------------
const char CIPSEND[] = "AT+CIPSEND=";
const char PROMPT[] = ">";
const char BUSY[] = "busy";
const char LINK_IS_NOT[] = "link is not";
const char SEND_OK[] = "SEND OK";
//------------------------------------------------------------------------------
void SerialESP8266wifi::send(char channel, const char* message) {
    if (message == nullptr) {
        return;
    }
    if (startSend(channel, strlen(message)) == false) {
        return;
    }
    endSend(channel, String(message));
}

bool SerialESP8266wifi::startSend(char channel, int size) {
    if (size <= 0) {
        return false;
    }
    watchdog();
    if (flags.endSendWithNewline) {
        size += 2;
    }
    writeCommand(CIPSEND);
    _serialOut->print(channel);
    writeCommand(COMMA);
    _serialOut->println(size);
    if (readCommand(1000, PROMPT, LINK_IS_NOT) != 2) {
        return true;
    }
    return false;
}

void SerialESP8266wifi::addSend(char channel, const String& message) {
    _serialOut->print(message);
}

void SerialESP8266wifi::endSend(char channel, const String& message) {
    if (flags.endSendWithNewline) {
        _serialOut->println(message);
    } else {
        _serialOut->print(message);
    }
}

void SerialESP8266wifi::writeCommand(const char* text1, const char* text2) {
    if (text1 != nullptr) {
        _serialOut->print(text1);
    }
    if (text2 == EOL) {
        _serialOut->println();
    } else if (text2 != nullptr) {
        _serialOut->print(text2);
    }
}

int SerialESP8266wifi::readCommand(int timeout, const char* text1, const char* text2, bool log, bool clear) {
    int len1 = 0, len2 = 0, pos1 = 0, pos2 = 0;
    if (text1 != nullptr) {
        len1 = strlen(text1);
    }
    if (text2 != nullptr) {
        len2 = strlen(text2);
    }
    char buff[65] = { 0 };
    unsigned long stop = millis() + timeout;
    if (clear == true) {
        recvcache.clear();
    }
    do {
        auto size = recvcache.length();
        if (size <= 0) {
            checkInput(log);
            if (rdchpos > 0) {
                recvcache.write(readcache, rdchpos);
                rdchpos = 0;
            }
        }
        size = recvcache.length();
        while (size > 0) {
            size = size > 64 ? 64 : size;
            recvcache.peek(buff, size);
            if (log) {
                showLog(buff);
            }
            for (auto i = 0; i < size; i++) {
                if (len1 > 0) {
                    pos1 = (buff[i] == *(text1 + pos1)) ? pos1 + 1 : 0;
                    if (pos1 == len1) {
                        recvcache.peekSize(i + 1);
                        return 1;
                    }
                }
                if (len2 > 0) {
                    pos2 = (buff[i] == *(text2 + pos2)) ? pos2 + 1 : 0;
                    if (pos2 == len2) {
                        recvcache.peekSize(i + 1);
                        return 2;
                    }
                }
            }
            recvcache.peekSize(size);
            size = recvcache.length();
        }
        niceDelay(10);
    } while (millis() < stop);
    return 0;
}

charFind SerialESP8266wifi::readBuffer(int timeout, const char* key, char* data, int dlen, char delim, bool log) {
    charFind fd;
    fd.find = false;
    fd.pos = 0;
    if (data == nullptr || dlen <= 0) {
        return fd;
    }
    int keylen = 0, poskey = 0, cpindex = 0, cplen = 0;
    char buff[65] = { 0 };
    if (key != nullptr) {
        keylen = strlen(key);
        cpindex = -1;
    }
    char c;
    unsigned long stop = millis() + timeout;
    do {
        auto size = recvcache.length();
        if (size <= 0) {
            checkInput(log);
            if (rdchpos > 0) {
                recvcache.write(readcache, rdchpos);
                rdchpos = 0;
            }
        }
        size = recvcache.length();
        while (size > 0) {
            size = size > 64 ? 64 : size;
            recvcache.peek(buff, size);
            buff[size] = 0;
            //showLog(String("readBuffer: ") + buff);
            for (auto i = 0; i < size; i++) {
                c = buff[i];
                if (keylen > 0) {
                    poskey = (c == *(key + poskey)) ? poskey + 1 : 0;
                    if (poskey >= keylen) {
                        cpindex = i;
                        continue;
                    }
                }
                if (cpindex >= 0) {
                    if (delim == 0 || delim != c) {
                        *(data + cplen++) = c;
                    } else {
                        recvcache.peekSize(i + 1);
                        fd.find = true;
                        fd.pos = cplen;
                        return fd;
                    }
                }
                if (dlen > 0 && cplen >= dlen) {
                    recvcache.peekSize(i + 1);
                    fd.pos = cplen;
                    return fd;
                }
            }
            recvcache.peekSize(size);
            size = recvcache.length();
        }
        niceDelay(10);
    } while (millis() < stop);
    fd.pos = cplen;
    return fd;
}

void SerialESP8266wifi::showLog(const String& info) {
    if (_dbgSerial != nullptr) {
        _dbgSerial->println(info);
    }
}

void SerialESP8266wifi::showChar(char c) {
    if (_dbgSerial != nullptr) {
        _dbgSerial->print(c);
    }
}